#include "backendshen/edgeshen.h"
#include "backendshen/vertexshen.h"
#include <iostream>

using namespace std;
namespace myslam
{
    namespace backend
    {
        unsigned long global_edge_id = 0;
        Edge::Edge(int residual_dimension,
                   int num_verticies,
                   const std::vector<std::string> &verticies_types)
        {
            residual_.resize(residual_dimension, 1);
            // verticies_.resize(num_verticies);  有点问题
            if (!verticies_types.empty())
            {
                verticies_types_ = verticies_types;
            }
            jacobins_.resize(num_verticies);
            id_ = global_edge_id++;
            std::cout << "edge id_:" << id_ << std::endl;
            Eigen::MatrixXd information(residual_dimension, residual_dimension);
            information.setIdentity();
            information_ = information;
        }
        Edge::~Edge()
        {
        }
        double Edge::Chi2()
        {
            return residual_.transpose() * information_ * residual_;
        }
        bool Edge::CheckValid()
        {
            if (!verticies_types_.empty())
            {
                // check type info
                for (size_t i = 0; i < verticies_.size(); i++)
                {
                    if (verticies_types_[i] != verticies_[i]->TypeInfo())
                    {
                        cout << "Vertex type does not match ,should be " << verticies_types_[i]
                             << ",but set to " << verticies_[i]->TypeInfo() << endl;
                        return false;
                    }
                }
            }
            return true;
        }
    }
}